//
// Created by jiangbin on 4/22/21.
//

#include <stdio.h>
#include <stdlib.h>
#include "../include/robot_arm.h"

const char arm_ctl_dir[][150] = {
        {"/sys/class/pwm/pwmchip6/pwm0/duty_cycle"},
        {"/sys/class/pwm/pwmchip3/pwm1/duty_cycle"},
        {"/sys/class/pwm/pwmchip3/pwm0/duty_cycle"},
};

static FILE *arm_fp[ARM_C_E + 1] = { NULL };

void robot_arm_init(void)
{
    int i = 0;
    for (i = 0; i < (ARM_C_E + 1); i++) {
        arm_fp[i] = fopen(arm_ctl_dir[i], "r+");
        if (arm_fp[i] == NULL)
            break;
    }

    if (i == (ARM_C_E + 1)) {
        printf("Robot Arm Init\n");
        return ;
    } else {
        while (i >= 0) {
            fclose(arm_fp[i]);
            i--;
        }
    }

    printf("Robot Arm Error\n");
    return ;
}

void robot_arm_exit(void)
{
    int i = 0;
    for (i = 0; i < (ARM_C_E + 1); i++) {
        if (arm_fp[i] != NULL) {
            fclose(arm_fp[i]);
            arm_fp[i] = NULL;
        }
    }
}

int robot_set_pos(int pos_l, int pos_r, int pos_c)
{
    int i = 0;

    if (pos_l < ARM_MIN_L || pos_l > ARM_MAX_L)
        pos_l = ARM_CENTER;

    if (pos_r < ARM_MIN_R || pos_r > ARM_MAX_R)
        pos_r = ARM_CENTER;

    if (pos_c < ARM_MIN_C || pos_c > ARM_MAX_C)
        pos_c = ARM_CENTER;

    for (i = 0; i < (ARM_C_E + 1); i++)
        fseek(arm_fp[i], 0L, SEEK_SET);

    pos_l = 2000 - pos_l;
    pos_r = 2000 - pos_r;
    pos_c = 2000 - pos_c;

    fprintf(arm_fp[ARM_L_E], "%d", pos_l * 10000);
    fprintf(arm_fp[ARM_R_E], "%d", pos_r * 10000);
    fprintf(arm_fp[ARM_C_E], "%d", pos_c * 10000);

    for (i = 0; i < (ARM_C_E + 1); i++)
        fflush(arm_fp[i]);

}

void arm_01(void)
{
    int pos_l, pos_r, pos_c;
    pos_l = pos_r = pos_c = 150;
    robot_arm_init();
    robot_set_pos(pos_l,pos_r,pos_c);
    printf("pos_l == %d pos_r == %d pos_c == %d\n",pos_l, pos_r, pos_c);
    robot_arm_exit();
}



